#include "STLWriter.h"

STLWriter::STLWriter() {
}

STLWriter::~STLWriter() {
}

/*!
 * Function to write collision info to an STL file
 * @param stlFileName String with the name of the STL file
 * @param width The width in mm of the boxes that are places at each point in the point cloud (in mm)
 * @param plane A vector of double of length 4 that carries (a,b,c,d), the plane equation: ax + by + cz + d = 0 (in mm)
 * @param points A vector containing arrays of double, each with the 3D position of a point (in mm)
 */
void STLWriter::writeCollisionFile(string stlFileName, std::vector<double> &plane, std::vector<double*> &points, double width) {
  Math::seed(time(NULL));
  srand ( time(NULL) );
  
  // first line is a plane
  Vector3D<> n(plane[0], plane[1], plane[2]);
  Plane p( normalize(n), ((plane[3])/n.norm2())*-0.001 ); // The normal (a,b,c) is normalized and d is transformed to meters

  // loop through all points
  std::vector<Vector3D<> > pointVectors;
  for(unsigned int i=0; i<points.size(); i++) {
    Vector3D<> v(points[i][0],points[i][1],points[i][2]);
    v = v*0.001; // The position in meters
    pointVectors.push_back(v);
  }

  // width in meters
  width *= 0.001;

  // now create the triangle mesh. Use boxes on the texlets
  Box pointBox(width,width,width);
  TriMesh::Ptr boxMesh = pointBox.getTriMesh();
  TriMesh::Ptr planeMesh = p.getTriMesh();
  size_t meshSize = planeMesh->size()+boxMesh->size()*pointVectors.size();
  PlainTriMesh<> stlMesh(meshSize);
  
  // and copy plane triangles into the mesh
  for(size_t i=0;i<planeMesh->size();i++)
    stlMesh[i] = planeMesh->getTriangle(i);
  
  // and copy every point into the mesh
  for(size_t i=0;i<pointVectors.size();i++){
    Transform3D<> transform(pointVectors[i], Rotation3D<>::identity());
    for(size_t j=0;j<boxMesh->size();j++){
      size_t idx = planeMesh->size()+i*boxMesh->size()+j;
      if(idx>=stlMesh.size())
	RW_THROW(idx<< ">=" << stlMesh.size() << "  i:" << i << " " << j);
      stlMesh[idx] = boxMesh->getTriangle(j).transform( transform );
    }
  }
  
  STLFile::save(stlMesh, stlFileName);
}
